#include "SG805Servo.h"

//crc16，Modbus校验
uint8_t crcHigh [256]= {
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
	0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
	0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
	0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
	0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
	0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
	0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
	0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
	0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
	0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
	0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
	0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
	0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};

uint8_t crcLow[256] = {
	0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
	0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
	0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
	0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
	0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
	0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
	0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
	0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
	0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
	0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
	0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
	0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
	0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
	0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
	0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
	0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
	0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
	0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
	0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
	0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
	0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
	0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
	0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
	0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
	0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
	0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};

uint16_t CRC16_MODBUS(uint8_t *_pBuf, int _usLen)
{
	uint8_t ucCRCHi = 0xFF; /* 高CRC字节初始化 */
	uint8_t ucCRCLo = 0xFF; /* 低CRC字节初始化 */
	uint16_t usIndex;		/* CRC循环中的索引 */

	while (_usLen--)
	{
		usIndex = ucCRCHi ^ *_pBuf++;		  //与8位帧数据异或后，再做移位运算，相当于做了计算上表数据的过程
		ucCRCHi = ucCRCLo ^ crcHigh[usIndex]; //此步可直接索引上表数据，但为什么不直接用索引数据，还要与ucCRCLo 异或？往下看
		ucCRCLo = crcLow[usIndex];
	}
	return ((uint16_t)ucCRCHi << 8 | ucCRCLo); //拼成2字节 16位
}

bool checkEncoderData(uint8_t *_pBuf, int _usLen)
{
	int crc16 = ((_pBuf[_usLen - 2] << 8) | _pBuf[ _usLen - 1]);
	int check = CRC16_MODBUS(_pBuf, _usLen-2);

	if (crc16 == check)
	{
		//std::cout << "校验成功" << std::endl;
		return true;
	}
	std::cout << "校验失败 " << crc16 << " " << check << std::endl;
	return false;
}




//伺服失能
//01 06 00 01 00 00
uint8_t disableServoCMD[6][8] = {
    {0x01, 0x06, 0x00, 0x01, 0x00, 0x00, 0xd8, 0x0a},    //失能
	{0x02, 0x06, 0x00, 0x01, 0x00, 0x00, 0xd8, 0x39},	
	{0x03, 0x06, 0x00, 0x01, 0x00, 0x00, 0xd9, 0xe8},
	{0x04, 0x06, 0x00, 0x01, 0x00, 0x00, 0xd8, 0x5f},
	{0x05, 0x06, 0x00, 0x01, 0x00, 0x00, 0xd9, 0x8e},
	{0x06, 0x06, 0x00, 0x01, 0x00, 0x00, 0xd9, 0xbd}
};


//伺服使能
//01 06 00 01 00 01
uint8_t enableServoCMD[6][8] = {
    {0x01, 0x06, 0x00, 0x01, 0x00, 0x01, 0x19, 0xca},   //使能
	{0x02, 0x06, 0x00, 0x01, 0x00, 0x01, 0x19, 0xf9},	
	{0x03, 0x06, 0x00, 0x01, 0x00, 0x01, 0x18, 0x28},
	{0x04, 0x06, 0x00, 0x01, 0x00, 0x01, 0x19, 0x9f},
	{0x05, 0x06, 0x00, 0x01, 0x00, 0x01, 0x18, 0x4e},
	{0x06, 0x06, 0x00, 0x01, 0x00, 0x01, 0x18, 0x7d}
};


//读伺服报警,获取错误状态
// 14硬件错误  8过流保护  4位置超差  7瞬时过流保护   2电压过低/加速太快/电源容量太小
// 20电压过高 17电压不稳定
// uint8_t alarmRequestCMD[6][8] = {
//     {0x01, 0x03, 0x00, 0x0e, 0x00, 0x01, 0x19, 0xca},
// 	{0x02, 0x03, 0x00, 0x0e, 0x00, 0x01, 0x00, 0x00},	
// 	{0x03, 0x03, 0x00, 0x0e, 0x00, 0x01, 0x00, 0x00},
// 	{0x04, 0x03, 0x00, 0x0e, 0x00, 0x01, 0x00, 0x00},
// 	{0x05, 0x03, 0x00, 0x0e, 0x00, 0x01, 0x00, 0x00},
// 	{0x06, 0x03, 0x00, 0x0e, 0x00, 0x01, 0x00, 0x00}
// };




//保存参数
// 01 06 00 14 00 01
uint8_t saveParamerCMD[7][8] = {
	{0x01, 0x06, 0x00, 0x14, 0x00, 0x01, 0x08, 0x0e},   //保存
	{0x02, 0x06, 0x00, 0x14, 0x00, 0x01, 0x08, 0x3d},
	{0x03, 0x06, 0x00, 0x14, 0x00, 0x01, 0x09, 0xec},
	{0x04, 0x06, 0x00, 0x14, 0x00, 0x01, 0x08, 0x5b},
	{0x05, 0x06, 0x00, 0x14, 0x00, 0x01, 0x09, 0x8a},
	{0x06, 0x06, 0x00, 0x14, 0x00, 0x01, 0x09, 0xb9},
	{0x00, 0x06, 0x00, 0x14, 0x00, 0x01, 0x09, 0xdf}   //广播保存参数
};


//设置ID
//00 06 00 15 00 01
uint8_t setIdCMD[6][8] = {
    {0x00, 0x06, 0x00, 0x15, 0x00, 0x01, 0x58, 0x1f},   //id设置1
	{0x00, 0x06, 0x00, 0x15, 0x00, 0x02, 0x18, 0x1e},   //id设置2
	{0x00, 0x06, 0x00, 0x15, 0x00, 0x03, 0xd9, 0xde},   //id设置3
	{0x00, 0x06, 0x00, 0x15, 0x00, 0x04, 0x98, 0x1c},   //id设置4
	{0x00, 0x06, 0x00, 0x15, 0x00, 0x05, 0x59, 0xdc},   //id设置5
	{0x00, 0x06, 0x00, 0x15, 0x00, 0x06, 0x19, 0xdd}    //id设置6
};


//设置电子齿轮比 分子 
// 01 06 00 0a 00 10 a8 04
uint8_t setNumeratorCMD[6][8] = {
	{0x01, 0x06, 0x00, 0x0a, 0x00, 0x10, 0xa8, 0x04},   //保存
	{0x02, 0x06, 0x00, 0x0a, 0x00, 0x10, 0xa8, 0x37},
	{0x03, 0x06, 0x00, 0x0a, 0x00, 0x10, 0xa9, 0xe6},
	{0x04, 0x06, 0x00, 0x0a, 0x00, 0x10, 0xa8, 0x51},
	{0x05, 0x06, 0x00, 0x0a, 0x00, 0x10, 0xa9, 0x80},
	{0x06, 0x06, 0x00, 0x0a, 0x00, 0x10, 0xa9, 0xb3}
};

//设置电子齿轮比 分母
// 01 06 00 0b 00 01 f9 c4
uint8_t setDenominatorCMD[6][8] = {
	{0x01, 0x06, 0x00, 0x0b, 0x00, 0x01, 0xf9, 0xc4},   //保存
	{0x02, 0x06, 0x00, 0x0b, 0x00, 0x01, 0xf9, 0xf7},
	{0x03, 0x06, 0x00, 0x0b, 0x00, 0x01, 0xf8, 0x26},
	{0x04, 0x06, 0x00, 0x0b, 0x00, 0x01, 0xf9, 0x91},
	{0x05, 0x06, 0x00, 0x0b, 0x00, 0x01, 0xf8, 0x40},
	{0x06, 0x06, 0x00, 0x0b, 0x00, 0x01, 0xf8, 0x73}
};


//移动到0点位置
// 01 06 00 16 00 00 68 0e
uint8_t return2zeroCMD[6][8] = {
    {0x01, 0x06, 0x00, 0x16, 0x00, 0x00, 0x68, 0x0e},   //id设置1
	{0x02, 0x06, 0x00, 0x15, 0x00, 0x02, 0x68, 0x3d},   //id设置2
	{0x03, 0x06, 0x00, 0x15, 0x00, 0x03, 0x58, 0x1f},   //id设置3
	{0x00, 0x06, 0x00, 0x15, 0x00, 0x04, 0x58, 0x1f},   //id设置4
	{0x00, 0x06, 0x00, 0x15, 0x00, 0x05, 0x58, 0x1f},   //id设置5
	{0x00, 0x06, 0x00, 0x15, 0x00, 0x06, 0x58, 0x1f}    //id设置6
};

//获取伺服版本号
// 01 03 00 db 00 02 
uint8_t versionRequestCMD[6][8] = {
	{0x01, 0x03, 0x00, 0xdb, 0x00, 0x02, 0xb4, 0x30},
	{0x02, 0x03, 0x00, 0xdb, 0x00, 0x02, 0xb4, 0x03},
	{0x03, 0x03, 0x00, 0xdb, 0x00, 0x02, 0xb5, 0xd2},
	{0x04, 0x03, 0x00, 0xdb, 0x00, 0x02, 0xb4, 0x65},
	{0x05, 0x03, 0x00, 0xdb, 0x00, 0x02, 0xb5, 0xb4},
	{0x06, 0x03, 0x00, 0xdb, 0x00, 0x02, 0xb5, 0x87}
};




//获取编码器数据
// 01 03 00 da 00 02
// 01 03 01 28 00 02
uint8_t positionRequestCMD[6][8] = {
    {0x01, 0x03, 0x00, 0xda, 0x00, 0x02, 0xe5, 0xf0},
    {0x02, 0x03, 0x00, 0xda, 0x00, 0x02, 0xe5, 0xc3},
	{0x03, 0x03, 0x00, 0xda, 0x00, 0x02, 0xe4, 0x12},
	{0x04, 0x03, 0x00, 0xda, 0x00, 0x02, 0xe5, 0xa5},
	{0x05, 0x03, 0x00, 0xda, 0x00, 0x02, 0xe4, 0x74},
	{0x06, 0x03, 0x00, 0xda, 0x00, 0x02, 0xe4, 0x47}
	//{0x01, 0x03, 0x01, 0x28, 0x00, 0x02, 0x45, 0xff},
	// {0x02, 0x03, 0x01, 0x28, 0x00, 0x02, 0x45, 0xcc},
	// {0x03, 0x03, 0x01, 0x28, 0x00, 0x02, 0x44, 0x1d},
	// {0x04, 0x03, 0x01, 0x28, 0x00, 0x02, 0x45, 0xaa},
	// {0x05, 0x03, 0x01, 0x28, 0x00, 0x02, 0x44, 0x7b},
	// {0x06, 0x03, 0x01, 0x28, 0x00, 0x02, 0x44, 0x48}
};

//多圈圈数清零 有效位数22位，圈数9位，分辨率15位
//圈数 512   分辨率8192(即细分)
//01 06 01 26 00 00 
uint8_t distanceRequestCMD[6][8] = {
	{0x50, 0x03, 0x00, 0x34, 0x00, 0x01, 0xc8, 0x45},		//返回7位数据
	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
};


uint8_t resetTurnCMD[6][8] = {
	{0x01, 0x06, 0x01, 0x26, 0x00, 0x00, 0x69, 0xfd},
	{0x02, 0x06, 0x01, 0x26, 0x00, 0x00, 0x69, 0xce},
	{0x03, 0x06, 0x01, 0x26, 0x00, 0x00, 0x68, 0x1f},
	{0x04, 0x06, 0x01, 0x26, 0x00, 0x00, 0x69, 0xa8},
	{0x05, 0x06, 0x01, 0x26, 0x00, 0x00, 0x68, 0x79},
	{0x06, 0x06, 0x01, 0x26, 0x00, 0x00, 0x68, 0x4a}
};


// 01 编码格式切换
// 01 06 00 ff 00 01 
// 01 06 01 0e 00 01 
// uint8_t resetTurnCMD[6][8] = {
// 	{0x01, 0x06, 0x00, 0xff, 0x00, 0x01, 0x78, 0x3a},
// 	{0x01, 0x06, 0x01, 0x0e, 0x00, 0x01, 0x28, 0x35},
// 	{0x03, 0x06, 0x01, 0x26, 0x00, 0x00, 0x68, 0x1f},
// 	{0x04, 0x06, 0x01, 0x26, 0x00, 0x00, 0x69, 0xa8},
// 	{0x05, 0x06, 0x01, 0x26, 0x00, 0x00, 0x68, 0x79},
// 	{0x06, 0x06, 0x01, 0x26, 0x00, 0x00, 0x68, 0x4a}
// };



// //检测编码器状态
// int getEncoderAlarm(uint8_t *_pBuf)
// {

// }

//编码器ID
int getEncoderID(uint8_t *_pBuf)
{
	return (int)_pBuf[0];
}

//多圈编码器，圈数获取
int getEncoderTurns(int position)
{
	//9位
	int turns;
	// position > 0
	// 	/32768

	// 	< 0
	// 	/32768  -1
	if (position >= 0)
	{
		turns = position / 32768;
	}
	else
	{
		turns = position / 32768 - 1;
	}
	return turns;
}

//单圈编码器位置获取
int getEncoderAngle(int position)
{
	//13位
	int angle;
	// >0
	// /32768 ...
	// <0
	// /32768 ...-A
	// -A + 32768

	if (position >= 0)
	{
		angle = position % 32768;
	}
	else
	{
		angle = (position % 32768) + 32768;
	}
	return angle;
}

//编码器位置获取
int getEncoderPosition(uint8_t *_pBuf)
{	
	//32位系统32位，64位系统64位。有效位数22位，圈数9位，分辨率15位
	//圈数 512   分辨率4096(即细分)
	//9 15 = 22位
	int temp_position = ((int32_t)_pBuf[5] << 24 | (int32_t)_pBuf[6] << 16 | (int32_t)_pBuf[3] << 8 | (int32_t)_pBuf[4] );	
	return temp_position;
}

//版本号获取 01 03 00 db 00 02 B4 30



// 50 03 00 34 00 02 88 44 
// 50 03 04 00 83 00 00 4B 1E 
// 50 03 00 34 00 01 c8 45 
// 50  3   2   2   95  84   87
// id  读  长   数  数  CRC16-反置
// 距离获取
int getDistance(uint8_t *_pBuf)
{
	int temp_distance = (int16_t)_pBuf[3] << 8 | (int16_t)_pBuf[4];
	return temp_distance;
}
